#include "Team.h"
#include "World.h"
#include "Remote.h"
#include "AttractiveField.h"
#include "RepulseField.h"
#include "TangentField.h"
#include "Vector2.h"
#include "RandomField.h"

#define _USE_MATH_DEFINES
#include <math.h>

#include <iostream>
using namespace std;

Team* Team::_instance = 0;

double normalize_angle(double angle);

Team::Team(void)
{
}

Team::~Team(void)
{
}

Team* Team::instance() {
	if (!_instance)
		_instance = new Team();

	return _instance;
}

void Team::update() {
	World* world = World::instance();
	BZRC* com = Remote::instance();

	vector <RepulseField> repulse_fields;
	vector <TangentField> tangential_fields;
	commands.clear();

	for (int i = 0; i < world->tanks().size(); i++) {

		tank_t* tank = &world->tanks().at(i);
		flag_t* flag = &world->flags().at(2);

		Vector2 flag_pos(flag->pos[0], flag->pos[1]);
		Vector2 tank_loc(tank->pos[0], tank->pos[1]);

		Vector2 base_pos;

		for(int i = 0; i < 4; i ++){
			base_pos.x += world->teams().at(3).base_corner[i][0];
			base_pos.x += world->teams().at(3).base_corner[i][1];
		}
		base_pos.x /= 4.0;
		base_pos.y /= 4.0;

		repulse_fields.clear();
		for (int i = 0; i < world->other_tanks().size(); i++) {
			otank_t* otank = &world->other_tanks().at(i);
			Vector2 otank_loc(otank->pos[0], otank->pos[1]);
			repulse_fields.push_back(RepulseField(otank_loc));
		}

		tangential_fields.clear();
		for(int i = 0; i < world->obstacles().size(); i ++){
			obstacle_t* obs = &world->obstacles().at(i);
			tangential_fields.push_back(TangentField(obs));
		}

		AttractiveField* field;
		AttractiveField flag_field(flag_pos);
		AttractiveField base_field(base_pos);

		if (tank->flag == "-") {
			field = &flag_field;
		} else {
			field = &base_field;
		}

		double tmag = 40;

		Vector2 attract_trajectory = field->get_trajectory(tank_loc);
		double attract_mag = attract_trajectory.magnitude();
		double speed = 0;
		if (attract_mag > tmag) {
			speed = 1;
		} else {
			speed = attract_mag / tmag;
		}

		if (attract_trajectory.magnitude() > 0)
			attract_trajectory = attract_trajectory.normalize();

		Vector2 repulse_trajectory;

		for (int i = 0; i < repulse_fields.size(); i++) {
			repulse_trajectory += repulse_fields.at(i).get_trajectory(tank_loc);
		}

		double repulse_scale = 1.0f;
		if (repulse_trajectory.magnitude() > 0)
			repulse_trajectory = repulse_trajectory.normalize() * repulse_scale;

		Vector2 tangential_trajectory;

		for (int i = 0; i < tangential_fields.size(); i++) {
			tangential_trajectory += tangential_fields.at(i).get_trajectory(tank_loc);
		}

		double tangential_scale = 2.0f;
		if(tangential_trajectory.magnitude() != 0)  
			tangential_trajectory = tangential_trajectory.normalize() * tangential_scale;

		RandomField random_field;
		Vector2 random_trajectory = random_field.get_trajectory(tank_loc);

		Vector2 trajectory = attract_trajectory + repulse_trajectory + tangential_trajectory + random_trajectory;

		double dtheta = trajectory.angle() - normalize_angle(tank->angle);
		double ttheta = M_PI / 2.0f;

		if(dtheta > M_PI)
			dtheta -= 2.0f*M_PI;
		else if(dtheta < -M_PI)
			dtheta += 2.0f*M_PI;

		double angvel = 0;
		if (dtheta > ttheta) {
			angvel = 1;
		} else if (abs(dtheta) > ttheta) {
			angvel = -1;
		}
		else {
			angvel = dtheta / ttheta;
		}

		Vector2 tank_cur_trajectory(cos(tank->angle), sin(tank->angle));

		double dot = Vector2::dot(trajectory, tank_cur_trajectory);

		if (attract_mag < 2 * tmag) {
			speed *= 1.0 / (2.0 - dot);
		}

		bool shoot = false;
		commands.push_back(Command(tank->index, speed, angvel, shoot));
	}
	com->do_commands(commands);
}

double normalize_angle(double angle) {
	if (angle > 0) {
		while (angle > 2 * M_PI) {
			angle -= 2 * M_PI;
		}
	} else {
		while (angle < 0) {
			angle += 2 * M_PI;
		}
	}

	return angle;
}